######################################################################## 
# 
# Author: Krzysztof Lazarski  
# Date: 20230720  
# 
# simple UR robot GUI with camera view 
# 
# 
########################################################################

from PyQt5 import QtCore, QtGui, QtWidgets 
from PyQt5.QtWidgets import *
from PyQt5.QtCore import Qt, QTimer, QTime 
from simpleRobotGUI_v1_ui import Ui_simpleRobotGUI  

import rtde_control 
import rtde_receive 
import robotiq_gripper 
import time 
import traceback 

import pyjoystick 
from pyjoystick.sdl2 import Key, Joystick, run_event_loop 
from qt_thread_updater import ThreadUpdater 

import cv2
import numpy as np
from imageio.v3 import imread 
import csv 
import sys 

# rtde_c = rtde_control.RTDEControlInterface("164.54.129.65") 
rtde_r = rtde_receive.RTDEReceiveInterface("164.54.129.65") 
init_q = rtde_r.getActualQ() 
x_act, y_act, z_act, rx_act, ry_act, rz_act = rtde_r.getActualTCPPose() 
x_init, y_init, z_init, rx_init, ry_init, rz_init = [-0.001051747, -0.4220357447, 0.1500377107, +2.2364672, +2.1999581, -0.0347437] 
# x_init, y_init, z_init, rx_init, ry_init, rz_init = rtde_r.getActualTCPPose() 
# init_q = [1.2493692636489868, -1.131822184925415, 0.9949505964862269, -1.4343610417893906, -1.570045296345846, 1.2711894512176514] 
joint_pos = [0.8154136538505554, -1.632439752618307, 1.6060140768634241, -1.562970184986927, -1.5679143110858362, 0.8337597250938416] 
joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
tcp_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
acceleration = 0.5 
dt = 1.0/500 
### t_start = rtde_c.initPeriod() 
# rtde_c.stopScript() 
# rtde_c.disconnect() 
controlFlag = False 
gamepadMode = "JOINTS" 
gamepadMode = "TCP"


class simpleRobotGUI(QDialog): 
	
	global init_q 
	global rtde_receive 
	global rtde_control
	global rtde_c, rtde_r 
	global x_act, y_act, z_act, rx_act, ry_act, rz_act 
	global x_init, y_init, z_init, rx_init, ry_init, rz_init 
	global joint_speed, tcp_speed, acceleration, dt 
	global joint_pos, t_start 
	global controlFlag 
	global gamepadMode 
	
	def __init__(self, parent=None): 
		super(simpleRobotGUI, self).__init__()

		# 
		self.ui = Ui_simpleRobotGUI()
		self.ui.setupUi(self) 
		
		# 
		self.ctimer = QtCore.QTimer() 
		self.ctimer.timeout.connect(self.ctimerUpdate) 
		self.ctimer.start(100) 
		self.mtimer = QtCore.QTimer() 
		self.mtimer.timeout.connect(self.mtimerUpdate) 
		self.mtimer.start(500) 
		self.camtimer = QtCore.QTimer() 
		self.camtimer.timeout.connect(self.camViewUpdate) 
		self.camtimer.start(250) 
		self.speedVal = 0.045 
		self.acceleration = 0.5 
		self.gamepadTCP = True 
		self.gamepadJOINTS = False 
		self.gripper = robotiq_gripper.RobotiqGripper() 
		# self.ui.lbKeys.setText("Gamepad/Joystick controls JOINTS movements") 
		self.ui.lbKeys.setText("Gamepad/Joystick controls TCP (XYZ mode) movements") 
		self.dt = 1.0/500 
		self.ui.txtXraw.setText(str(format(x_act,"+010.7f"))) 
		self.ui.txtYraw.setText(str(format(y_act,"+010.7f"))) 
		self.ui.txtZraw.setText(str(format(z_act,"+010.7f"))) 
		self.ui.txtRXraw.setText(str(format(rx_act,"+010.7f"))) 
		self.ui.txtRYraw.setText(str(format(ry_act,"+010.7f"))) 
		self.ui.txtRZraw.setText(str(format(rz_act,"+010.7f"))) 
		x_mm_act = 1000 * x_init - 100.0 
		y_mm_act = 1000 * y_init + 120.0  
		z_mm_act = 1000 * z_init 
		rx_mm_act = rx_init 
		ry_mm_act = ry_init 
		rz_mm_act = rz_init
		self.ui.txtXmmRqs.setText(str(format(x_mm_act,"+010.5f"))) 
		self.ui.txtYmmRqs.setText(str(format(y_mm_act,"+010.5f"))) 
		self.ui.txtZmmRqs.setText(str(format(z_mm_act,"+010.5f"))) 
		self.ui.txtRXmmRqs.setText(str(format(rx_mm_act,"+010.7f"))) 
		self.ui.txtRYmmRqs.setText(str(format(ry_mm_act,"+010.7f"))) 
		self.ui.txtRZmmRqs.setText(str(format(rz_mm_act,"+010.7f"))) 

		self.ui.lbStatus.setStyleSheet("background-color: yellow; border: 1px solid black;") 
		# 
		# self.ui.btAdd.clicked.connect(self.fAdd) 
		self.ui.btQuit.clicked.connect(self.fQuit) 
		self.ui.btGO.clicked.connect(self.fGO) 
		self.ui.btGOinit.clicked.connect(self.fGOinit) 
		self.ui.btRecordPos.clicked.connect(self.fRecordPos) 
		self.ui.btRecordQPos.clicked.connect(self.fRecordQPos) 
		self.ui.btGRPactivate.clicked.connect(self.fGRPactivate) 
		self.ui.btGRPmove.clicked.connect(self.fGRPmove) 
		self.ui.btGRPopen.clicked.connect(self.fGRPopen) 
		
		# self.ui.btJ1plus.clicked.connect(self.fJ1plus_clicked) 
		# self.ui.btJ1plus.pressed.connect(self.fJ1plus_pressed) 
		# self.ui.btJ1plus.released.connect(self.fJ1plus_released) 
		# self.ui.btJ1minus.clicked.connect(self.fJ1minus_clicked) 
		# self.ui.btJ1minus.pressed.connect(self.fJ1minus_pressed) 
		# self.ui.btJ1minus.released.connect(self.fJ1minus_released) 
		
		self.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
		self.ui.btInitControl.clicked.connect(self.fInitControl) 
		self.ui.btStopControl.clicked.connect(self.fStopControl) 
		self.ui.btJ1plus.clicked.connect(self.fJ1plus) 
		self.ui.btJ1minus.clicked.connect(self.fJ1minus) 
		self.ui.btJ1stop.clicked.connect(self.fJ1stop) 
		self.J1stop = True 
		self.J1_timer = QtCore.QTimer() 
		self.J1_timer.timeout.connect(self.J1_timerUpdate) 
		# 
		# 
		self.ui.model = QtGui.QStandardItemModel(self) 
		self.ui.tableView.setModel(self.ui.model) 
		# self.tableView.clicked['QModelIndex'].connect(simpleRobotGUI.select_onclick)
		# QtCore.QMetaObject.connectSlotsByName(simpleRobotGUI)
		self.ui.tableView.clicked.connect(self.tbl_select_onclick) 
		fileName = 'robot_init_positions.csv' 
		with open(fileName, "r") as fileInput: 
			for row in csv.reader(fileInput): 
				items = [
					QtGui.QStandardItem(field)
					for field in row
				]
				self.ui.model.appendRow(items) 
		self.updater = ThreadUpdater() 
		self.repeater = pyjoystick.HatRepeater(first_repeat_timeout=0.5, repeat_timeout=0.03, check_timeout=0.01) 
		self.mngr = pyjoystick.ThreadEventManager(event_loop=run_event_loop, handle_key_event=self.handle_key_event, button_repeater=self.repeater) 
		self.mngr.start() 
		
		
	def fQuit(arg): 
		print("Closing the GUI ... ") 
		try: 
			if(rtde_c.isConnected()): 
				rtde_c.stopScript() 
				rtde_c.disconnect() 
		except:
			pass 
		try:
			if(rtde_r.isConnected()): 
				rtde_r.disconnect() 
		except: 
			pass 
		try: 
			if(arg.rtde_c.isConnected()): 
				arg.rtde_c.stopScript() 
				arg.rtde_c.disconnect() 
		except:
			pass 
		exit() 
		sys._exit() 
		
	
	def fGRPactivate(arg): 
		# arg.gripper = robotiq_gripper.RobotiqGripper() 
		arg.gripper.connect("164.54.129.65", 63352) 
		arg.gripper.activate(auto_calibrate=True) 
		
	def fGRPmove(arg): 
		rqs_pos = int(arg.ui.txtGRPvalue.text()) 
		print("rqs_pos: ", str(rqs_pos), "min_pos: ", arg.gripper.get_max_position()) 
		if(rqs_pos<arg.gripper.get_max_position()): 
			arg.gripper.move(rqs_pos,255,255) 
		else: 
			arg.gripper.move(arg.gripper.get_max_position(),255,255) 
		
	def fGRPopen(arg): 
		print("max_pos: ", arg.gripper.get_min_position()) 
		arg.gripper.move(arg.gripper.get_min_position(),255,255) 
	
	
	def handle_key_event(arg,key):
		# arg.updater.now_call_latest(arg.ui.lbKeys.setText, '{}: {} = {}'.format(key.joystick, key, key.value)) 
		# print(key, '-', key.keytype, '-', key.number, '-', key.value)
		print('{}: Key: {} KeyType: {} KeyNumber: {} = {}'.format(key.joystick, key, key.keytype, key.number, key.value)) 
		
		if key.keytype=="Button" and key.number == 3 and key.value==1: 
			arg.gamepadTCP = False 
			arg.gamepadJOINTS = True 
			gamepadMode = "JOINTS" 
			arg.ui.lbKeys.setText("Gamepad/Joystick controls JOINTS movements") 
		elif key.keytype=="Button" and key.number == 2 and key.value==1: 
			arg.gamepadTCP = True 
			arg.gamepadJOINTS = False 
			gamepadMode = "TCP" 
			arg.ui.lbKeys.setText("Gamepad/Joystick controls TCP (XYZ mode) movements") 
		elif arg.gamepadTCP and key.keytype=="Axis" and key.number == 1 and key.value<-0.5: 
			# print("a") 
			# print("X+") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[0] = +(arg.speedVal) 
			arg.rtde_c.speedL(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadTCP and key.keytype=="Axis" and key.number == 1 and key.value>0.5: 
			# print("s") 
			# print("X-") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[0] = -(arg.speedVal) 
			arg.rtde_c.speedL(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadTCP and key.keytype=="Axis" and key.number == 0 and key.value<-0.5: 
			# print("w") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[1] = +(arg.speedVal) 
			arg.rtde_c.speedL(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadTCP and key.keytype=="Axis" and key.number == 0 and key.value>0.5: 
			# print("d") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[1] = -(arg.speedVal) 
			arg.rtde_c.speedL(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif key.keytype=="Button" and key.number == 4 and key.value == 1: 
			# print("SafeHome") 
			safe_q = [1.2534008026123047, -1.1563940805247803, 1.040295426045553, -1.476695367400982, -1.5765131155597132, 1.2723886966705322] 
			arg.rtde_c.moveJ(safe_q) 
		elif key.keytype=="Button" and key.number == 5 and key.value == 1: 
			# print("pos0") 
			pos0_q = [0.8185794353485107, -1.6313044033446253, 1.609772030507223, -1.571996351281637, -1.5660799185382288, 0.8374824523925781]
			arg.rtde_c.moveJ(pos0_q) 
		elif arg.gamepadTCP and key.keytype=="Axis" and key.number == 4 and key.value<-0.5: 
			# print("z") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[2] = +(arg.speedVal) 
			arg.rtde_c.speedL(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadTCP and key.keytype=="Axis" and key.number == 4 and key.value>0.5: 
			# print("x") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[2] = -(arg.speedVal) 
			arg.rtde_c.speedL(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		# '''
		# ******* JOINTS *******
		# ''' 
		elif arg.gamepadJOINTS and key.keytype=="Axis" and key.number == 3 and key.value<-0.5: 
			# print("k") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[0] = +(arg.speedVal+0.025) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadJOINTS and key.keytype=="Axis" and key.number == 3 and key.value>0.5: 
			# print("l") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[0] = -(arg.speedVal+0.025) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadJOINTS and key.keytype=="Axis" and key.number == 4 and key.value<-0.5: 
			# print("o") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[1] = +(arg.speedVal+0.025) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadJOINTS and key.keytype=="Axis" and key.number == 4 and key.value>0.5: 
			# print("p") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[1] = -(arg.speedVal+0.025) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadJOINTS and key.keytype=="Axis" and key.number == 0 and key.value<-0.5: 
			# print("n") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[2] = +(arg.speedVal+0.025) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadJOINTS and key.keytype=="Axis" and key.number == 0 and key.value>0.5: 
			# print("m") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[2] = -(arg.speedVal+0.025) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadJOINTS and key.keytype=="Axis" and key.number == 1 and key.value<-0.5: 
			# print("9") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[3] = +(arg.speedVal+0.025) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadJOINTS and key.keytype=="Axis" and key.number == 1 and key.value>0.5: 
			# print("0") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[3] = -(arg.speedVal+0.025) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadJOINTS and key.keytype=="Hat" and key.value==2: 
			# print("h") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[4] = +(arg.speedVal+0.025) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadJOINTS and key.keytype=="Hat" and key.value==8: 
			# print("j") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[4] = -(arg.speedVal+0.025) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadJOINTS and key.keytype=="Hat" and key.value==1: 
			# print("v") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[5] = +(arg.speedVal+0.025) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif arg.gamepadJOINTS and key.keytype=="Hat" and key.value==4: 
			# print("b") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[5] = -(arg.speedVal+0.025) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		else: 
			try: 
				arg.rtde_c.speedStop(arg.acceleration) 
				arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			except: 
				# print(traceback.format_exc()) 
				pass 
		'''

		else: 
			arg.rtde_c.speedStop(arg.acceleration) 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
		'''
		
		'''
		if key.keytype != Key.HAT:
			return
	
		if key.value == Key.HAT_UP:
			# mover.point.setY(mover.point.y() - 10) 
			print(key, '-', key.keytype, '-', key.number, '-', key.value) 
		elif key.value == Key.HAT_DOWN:
			# mover.point.setY(mover.point.y() + 10)
			print(key, '-', key.keytype, '-', key.number, '-', key.value) 
		if key.value == Key.HAT_LEFT:
			# mover.point.setX(mover.point.x() - 10)
			print(key, '-', key.keytype, '-', key.number, '-', key.value) 
		elif key.value == Key.HAT_UPLEFT:
			# mover.point.setX(mover.point.x() - 5)
			# mover.point.setY(mover.point.y() - 5)
			print(key, '-', key.keytype, '-', key.number, '-', key.value) 
		elif key.value == Key.HAT_DOWNLEFT:
			# mover.point.setX(mover.point.x() - 5)
			# mover.point.setY(mover.point.y() + 5)
			print(key, '-', key.keytype, '-', key.number, '-', key.value) 
		elif key.value == Key.HAT_RIGHT:
			# mover.point.setX(mover.point.x() + 10)
			print(key, '-', key.keytype, '-', key.number, '-', key.value) 
		elif key.value == Key.HAT_UPRIGHT:
			# mover.point.setX(mover.point.x() + 5)
			# mover.point.setY(mover.point.y() - 5)
			print(key, '-', key.keytype, '-', key.number, '-', key.value) 
		elif key.value == Key.HAT_DOWNRIGHT:
			# mover.point.setX(mover.point.x() + 5)
			# mover.point.setY(mover.point.y() + 5)
			print(key, '-', key.keytype, '-', key.number, '-', key.value) 
		# updater.now_call_latest(mover.move, mover.point) 
		'''
		# arg.updater.now_call_latest(arg.ui.lbKeys.setText, '{}: {} = {}'.format(key.joystick, key, key.value)) 
	

	def keyPressEvent(arg, e): 
		if e.key() == Qt.Key_F3:
			print(1)
		elif e.key() == Qt.Key_F4:
			print(0) 
		elif e.key() == 65: 
			# print("a") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[0] = +(arg.speedVal) 
			arg.rtde_c.speedL(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 83: 
			# print("s") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[0] = -(arg.speedVal) 
			arg.rtde_c.speedL(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 87: 
			# print("w") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[1] = +(arg.speedVal) 
			arg.rtde_c.speedL(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 68: 
			# print("d") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[1] = -(arg.speedVal) 
			arg.rtde_c.speedL(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 81: 
			# print("q") 
			arg.rtde_c.speedStop(arg.acceleration) 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
		elif e.key() == 67: 
			# print("c") 
			arg.rtde_c.speedStop(arg.acceleration) 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
		elif e.key() == 90: 
			# print("z") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[2] = +(arg.speedVal) 
			arg.rtde_c.speedL(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 88: 
			# print("x") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[2] = -(arg.speedVal) 
			arg.rtde_c.speedL(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 75: 
			# print("k") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[0] = +(arg.speedVal) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 76: 
			# print("l") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[0] = -(arg.speedVal) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 79: 
			# print("o") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[1] = +(arg.speedVal) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 80: 
			# print("p") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[1] = -(arg.speedVal) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 78: 
			# print("n") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[2] = +(arg.speedVal) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 77: 
			# print("m") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[2] = -(arg.speedVal) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 57: 
			# print("9") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[3] = +(arg.speedVal) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 48: 
			# print("0") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[3] = -(arg.speedVal) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 72: 
			# print("h") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[4] = +(arg.speedVal) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 74: 
			# print("j") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[4] = -(arg.speedVal) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 86: 
			# print("v") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[5] = +(arg.speedVal) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 66: 
			# print("b") 
			arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
			acceleration = 0.5 
			dt = 1.0/500 
			t_start = arg.rtde_c.initPeriod() 
			print(t_start) 
			arg.joint_speed[5] = -(arg.speedVal) 
			arg.rtde_c.speedJ(arg.joint_speed, arg.acceleration, arg.dt) 
			arg.rtde_c.waitPeriod(t_start) 
		elif e.key() == 85: 
			# print("u") 
			arg.speedVal += 0.005 
		elif e.key() == 89: 
			# print("y") 
			arg.speedVal -= 0.005 
		else: 
			print("key pressed: ", chr(e.key()), "  ", str(int(e.key())) ) 
	
	
	
	def fGO(arg): 
		arg_control = False 
		controlFlag = False 
		try: 
			arg_control = arg.rtde_c.isConnected() 
			controlFlag = True 
		except: 
			print(traceback.format_exc()) 
			try: 
				rtde_c = rtde_control.RTDEControlInterface("164.54.129.65") 
				controlFlag = True 
			except: 
				print(traceback.format_exc()) 
				pass 
		if(arg_control): 
			controlFlag = True 
		else: 
			try: 
				rtde_c = rtde_control.RTDEControlInterface("164.54.129.65") 
				controlFlag = True 
			except: 
				print(traceback.format_exc()) 
				controlFlag = False 
				if(rtde_c.isConnected()): 
					controlFlag  = True 
				pass 
		if(controlFlag): 
			x_mm_rqs = float(arg.ui.txtXmmRqs.text()) 
			x_rqs = x_mm_rqs/1000.0 
			y_mm_rqs = float(arg.ui.txtYmmRqs.text()) 
			y_rqs = y_mm_rqs/1000.0 
			z_mm_rqs = float(arg.ui.txtZmmRqs.text()) 
			z_rqs = z_mm_rqs/1000.0 
			# print("XmmRqs:", x_mm_rqs, "X_Rqs:", x_rqs) 
			rx_mm_rqs = float(arg.ui.txtRXmmRqs.text()) 
			rx_rqs = rx_mm_rqs 
			ry_mm_rqs = float(arg.ui.txtRYmmRqs.text()) 
			ry_rqs = ry_mm_rqs 
			rz_mm_rqs = float(arg.ui.txtRZmmRqs.text()) 
			rz_rqs = rz_mm_rqs 
			print("MoveL to ", x_mm_rqs, " ", y_mm_rqs, " ", z_mm_rqs) 
			target = [x_rqs,y_rqs,z_rqs,rx_rqs,ry_rqs,rz_rqs] 
			if(arg_control): 
				arg.rtde_c.moveL(target, 0.25, 0.5, False) 
			else: 
				rtde_c.moveL(target, 0.25, 0.5, False) 
			print("Movement finished.") 
		try: 
			if(rtde_c.isConnected()): 
				rtde_c.stopScript() 
				rtde_c.disconnect() 
		except: 
			pass 
		try: 
			if(not(rtde_r.isConnected())): 
				rtde_r.reconnect() 
		except: 
			pass 
		# rtde_r = rtde_receive.RTDEReceiveInterface("164.54.129.65") 
		
	def fGOinit(arg): 
		arg_control = False 
		controlFlag = False 
		try: 
			arg_control = arg.rtde_c.isConnected() 
			controlFlag = True 
		except: 
			print(traceback.format_exc()) 
			try: 
				rtde_c = rtde_control.RTDEControlInterface("164.54.129.65") 
				controlFlag = True 
			except: 
				print(traceback.format_exc()) 
				pass 
		if(arg_control): 
			controlFlag = True 
		else: 
			try: 
				rtde_c = rtde_control.RTDEControlInterface("164.54.129.65") 
				controlFlag = True 
			except: 
				print(traceback.format_exc()) 
				controlFlag = False 
				if(rtde_c.isConnected()): 
					controlFlag  = True 
				pass 
		if(controlFlag): 
			target = [x_init,y_init,z_init,rx_init,ry_init,rz_init] 
			print("MoveL to ", x_init, " ", y_init, " ", z_init) 
			if(arg_control): 
				arg.rtde_c.moveL(target, 0.25, 0.5, False) 
			else: 
				rtde_c.moveL(target, 0.25, 0.5, False) 
			print("Movement finished.") 
		try: 
			if(rtde_c.isConnected()): 
				rtde_c.stopScript() 
				rtde_c.disconnect() 
		except: 
			pass 
		try: 
			if(not(rtde_r.isConnected())): 
				rtde_r.reconnect() 
		except: 
			pass 
		# rtde_r = rtde_receive.RTDEReceiveInterface("164.54.129.65") 
		
	
	def fRecordPos(arg): 
		actual_q = rtde_r.getActualQ() 
		actual_tcp = rtde_r.getActualTCPPose() 
		print(actual_q) 
		print(actual_tcp) 
	
	def fRecordQPos(arg): 
		actual_q = rtde_r.getActualQ() 
		actual_tcp = rtde_r.getActualTCPPose() 
		print(actual_q) 
		print(actual_tcp) 
	
	
	
	
	
	
	def fInitControl(arg): 
		print("InitControl") 
		try: 
			arg.rtde_c = rtde_control.RTDEControlInterface("164.54.129.65") 
			print("Control Interface initialized") 
		except: 
			print(traceback.format_exc()) 
			pass 
		arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
		arg.jogIncrement = +0.010 
	
	def fStopControl(arg): 
		print("StopControl") 
		try: 
			arg.rtde_c.speedStop() 
			arg.rtde_c.stopScript() 
			arg.rtde_c.disconnect() 
			print("Control Interface disconnected") 
		except: 
			print(traceback.format_exc()) 
			pass 
		
	
	def fJ1plus(arg): 
		arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
		arg.jogIncrement = +0.010 
		arg.J1_timer.start(2) 
		
	def J1_timerUpdate(arg): 
		acceleration = 0.5 
		dt = 1.0/500 
		t_start = arg.rtde_c.initPeriod() 
		print(t_start) 
		arg.joint_speed[0] = arg.jogIncrement  
		arg.rtde_c.speedL(arg.joint_speed, acceleration, dt) 
		arg.rtde_c.waitPeriod(t_start) 
		
	def fJ1stop(arg): 
		arg.J1_timer.stop() 
		
	def fJ1minus(arg): 
		arg.joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
		arg.jogIncrement = -0.010 
		arg.J1_timer.start(2) 
	
	
	
	
	def ffJ1plus_clicked(arg): 
		print("J1plus clicked") 
		try: 
			rtde_c = rtde_control.RTDEControlInterface("164.54.129.65") 
			controlFlag = True 
		except: 
			print(traceback.format_exc()) 
			pass 
		acceleration = 0.5 
		dt = 1.0/500 
		joint_speed[0] = +0.110 
		t_start = rtde_c.initPeriod() 
	
	def ffJ1plus_pressed(arg): 
		print("J1plus pressed") 
		joint_speed[0] = +0.110 
		rtde_c.speedL(joint_speed, acceleration, dt) 
		rtde_c.waitPeriod(t_start) 
		
	def ffJ1plus_released(arg): 
		print("J1plus released") 
		rtde_c.speedStop() 
		rtde_c.stopScript() 
		
	def ffJ1minus_clicked(arg): 
		print("J1minus clicked") 
		try: 
			rtde_c = rtde_control.RTDEControlInterface("164.54.129.65") 
			controlFlag = True 
		except: 
			print(traceback.format_exc()) 
			pass 
		acceleration = 0.5 
		dt = 1.0/500 
		joint_speed[0] = -0.110 
		t_start = rtde_c.initPeriod() 
	
	def ffJ1minus_pressed(arg): 
		print("J1minus pressed") 
		joint_speed[0] = -0.110 
		rtde_c.speedL(joint_speed, acceleration, dt) 
		rtde_c.waitPeriod(t_start) 
		
	def ffJ1minus_released(arg): 
		print("J1minus released") 
		rtde_c.speedStop() 
		rtde_c.stopScript() 
		
	def ctimerUpdate(arg): 
		time = QTime.currentTime() 
		arg.ui.lbHour.setText(str(time.hour()).zfill(2)) 
		arg.ui.lbMinute.setText(str(time.minute()).zfill(2)) 
		arg.ui.lbSecond.setText(str(time.second()).zfill(2)) 
		arg.ui.lbMSecond.setText(str(time.msec()).zfill(3)) 
		
	def camViewUpdate(arg): 
		imageReady = False  
		try: 
			arg.image = cv2.cvtColor(np.array(imread("http://164.54.129.65:4242/current.jpg?type=color")), cv2.COLOR_RGB2BGR) 
			imageReady = True
		except: 
			print(traceback.format_exc()) 
			imageReady = False 
			pass 
		if(imageReady): 
			arg.image = QtGui.QImage(arg.image.data, arg.image.shape[1], arg.image.shape[0], QtGui.QImage.Format_RGB888).rgbSwapped() 
			# arg.image = arg.image.scaled(480,360, aspectRatioMode=QtCore.Qt.KeepAspectRatio, transformMode=QtCore.Qt.SmoothTransformation) 
			arg.image = arg.image.scaled(arg.ui.camView.width(), arg.ui.camView.height(), aspectRatioMode=QtCore.Qt.KeepAspectRatio, transformMode=QtCore.Qt.SmoothTransformation) 
			arg.ui.camView.setPixmap(QtGui.QPixmap.fromImage(arg.image)) 
		
	def mtimerUpdate(arg): 
		try: 
			x_act, y_act, z_act, rx_act, ry_act, rz_act = rtde_r.getActualTCPPose() 
		except: 
			print(traceback.format_exc()) 
			pass 
		x_mm_act = 1000 * x_act 
		y_mm_act = 1000 * y_act 
		z_mm_act = 1000 * z_act 
		rx_mm_act = rx_act 
		ry_mm_act = ry_act 
		rz_mm_act = rz_act 
		# print("   X:", x_act, "   Y:", y_act, "   Z:", z_act, "   RTDE_state:", rtde_r.isConnected()) 
		if(rtde_r.isConnected()): 
			arg.ui.lbStatus.setText("Connected") 
		else: 
			arg.ui.lbStatus.setText("Disconnected") 
			rtde_r.reconnect() 
		arg.ui.txtXraw.setText(str(format(x_act,"+010.7f"))) 
		arg.ui.txtYraw.setText(str(format(y_act,"+010.7f"))) 
		arg.ui.txtZraw.setText(str(format(z_act,"+010.7f"))) 
		arg.ui.txtRXraw.setText(str(format(rx_act,"+010.7f"))) 
		arg.ui.txtRYraw.setText(str(format(ry_act,"+010.7f"))) 
		arg.ui.txtRZraw.setText(str(format(rz_act,"+010.7f"))) 
		arg.ui.txtXmmAct.setText(str(format(x_mm_act,"+010.5f"))) 
		arg.ui.txtYmmAct.setText(str(format(y_mm_act,"+010.5f"))) 
		arg.ui.txtZmmAct.setText(str(format(z_mm_act,"+010.5f"))) 
		arg.ui.txtRXmmAct.setText(str(format(rx_mm_act,"+010.7f"))) 
		arg.ui.txtRYmmAct.setText(str(format(ry_mm_act,"+010.7f"))) 
		arg.ui.txtRZmmAct.setText(str(format(rz_mm_act,"+010.7f"))) 
		
	def tbl_select_onclick(arg, index): 
		print("Selected: ", index.row(), " ", index.column()) 
		# self.ui.model ... 
		# self.datamodel.input_data[index.row()][index.column()] = data_i_want_to_insert 
		# self.datamodel.layoutChanged.emit()
		


if __name__ == "__main__":
	import sys
	app = QtWidgets.QApplication(sys.argv) 
	myapp = simpleRobotGUI() 
	myapp.show() 
	sys.exit(app.exec_()) 
	
	
